#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import cv2
import numpy as np
import sys
import math

from numpy.lib.twodim_base import vander
import rospy

# 8.26
# 这份代码用来进行巡线图片测试

# 透视变换用到的参数
global src_set
global dst_set
global M_Perspective
global image_HEIGHT
global image_WID
# --------------------
# 二值化形态形态学操作用到的卷积核
global kernel_set
# --------------------
# 进行巡线检测用到的参数
global mid_point

global left_boundary_445, left_flag_445
global right_boundary_445, right_flag_445

global Extra_lines

global White_points_dec_thresholds

global angle_list
global angle_len


# 进行参数初始化的函数
def init_lane_detect():
    # 透视变换用到的参数
    global src_set
    global dst_set
    global M_Perspective
    global image_HEIGHT
    global image_WID
    # --------------------
    # 二值化形态形态学操作用到的卷积核
    global kernel_set
    # --------------------
    # 进行巡线检测用到的参数
    global mid_point  # 图像的中线位置
    # 各个行的左右边界，以及左右点的计数标志，就是检测到一个边界点，计数标志就+1，直到阈值

    global left_boundary_445, left_flag_445
    global right_boundary_445, right_flag_445

    # 判断单线还是双线的时候，扫描的额外行数
    global Extra_lines
    # 判断边线边界的白色点检测阈值
    global White_points_dec_thresholds

    # 巡线滞后队列
    global angle_list
    # 滞后队列的长度
    global angle_len

    # ----------------------------------------------------------------------------透视变换需要用到的参数设置
    image_HEIGHT = 480
    image_WID = 640
    # 这里获取透视变换的矩阵
    # 首先需要设定好两组四个需要标定的点
    # 第一组目标点，需要你打开原图，找到四个你需要进行变换的坐标点，你有写好测试程序路径为/调试工具/Find the coordinates of the picture.py
    src_set = np.float32([[205, 219], [424, 219], [27, 340], [598, 344]])
    # 第二组标定点就比较简单，和上面你标定的点是对应的，你这里采用了Z字型的标定顺序
    dst_set = np.float32([[80, 0], [560, 0], [80, 480], [560, 480]])
    # 这里通过调用一个API来获得变换矩阵
    M_Perspective = cv2.getPerspectiveTransform(src=src_set, dst=dst_set)

    # -------------------------------------------------------------------------------形态学操作的矩阵
    # 生成用于形态学操作的矩阵
    kernel_set = cv2.getStructuringElement(shape=cv2.MORPH_RECT,
                                           ksize=(7, 7),
                                           anchor=(-1, -1)
                                           )
    # -------------------------------------------------------------------------------巡线参数设

    mid_point = 320

    # 额外扫描行数设置
    Extra_lines = 30

    # 判断边线边界的白色点检测阈值设为10，检测到10个点就认为这个是巡线的边界
    White_points_dec_thresholds = 5

    # 巡线的角度滞后列表
    angle_list = []
    # 巡线滞后队列长度
    angle_len = 3
    for i in range(angle_len):
        angle_list.append(0)


# 对赛道进行透视变换获得俯视图的函数
def warp_image(image_for_warp):
    global src_set
    global dst_set
    global M_Perspective
    global image_HEIGHT
    global image_WID

    # 下面是进行透视变换的操作
    wig_perspective_get = cv2.warpPerspective(src=image_for_warp,
                                              M=M_Perspective,
                                              dsize=(image_WID, image_HEIGHT)
                                              )

    # 返回透视变换的图片
    return wig_perspective_get


# 对图像进行二值化操作的函数(这里面包含了形态学的开操作来去除噪点)
def Binary_image(img_bina):
    global kernel_set

    Bina_image = img_bina
    # 将图像变为灰度图
    wig_Gray = cv2.cvtColor(Bina_image, cv2.COLOR_BGRA2GRAY)

    # 用threshold（手动/自动设置全局阈值）对灰度图进行二值化处理
    # ret, wig_Bina_set = cv2.threshold(src=wig_Gray,
    #                                   thresh=130,
    #                                   maxval=255,
    #                                   # 这里设计这个参数，其实是让函数自己找一个全局阈值了
    #                                   type=cv2.THRESH_BINARY | cv2.THRESH_OTSU)

    ret, wig_Bina_set = cv2.threshold(src=wig_Gray,
                                      thresh=130,
                                      maxval=255,
                                      # 这里设计这个参数，其实是让函数自己找一个全局阈值了
                                      type=cv2.THRESH_BINARY | cv2.THRESH_OTSU)

    # 对获得的图像进行形态学的开操作，去除黑色区域的白色噪点
    open_cal_get = cv2.morphologyEx(src=wig_Bina_set,
                                    op=cv2.MORPH_OPEN,
                                    kernel=kernel_set,
                                    iterations=2)

    return open_cal_get


# 找到线的道路线的大概边界
def boundary_detect(bina_img_for_lane_detect):
    # 进行巡线检测用到的参数
    global mid_point

    # 各个行的左右边界，以及左右点的计数标志，就是检测到一个边界点，计数标志就+1，直到阈值
    global left_boundary_445, left_flag_445
    global right_boundary_445, right_flag_445

    # 判断单线还是双线的时候，扫描的额外行数
    global Extra_lines

    # 判断边线边界的白色点检测阈值
    global White_points_dec_thresholds

    # 参数都初始化为0

    left_boundary_445 = 0
    left_flag_445 = 0

    right_boundary_445 = 0
    right_flag_445 = 0

    # 这里拷贝一份图片，用于扫描处理
    img_detect_line = bina_img_for_lane_detect

    # 从中间往两边进行扫描
    for i in range(mid_point):
        # 行的扫描

        # 向左扫描
        # 扫描445行+额外的30行
        if 255 in img_detect_line[445:445 + Extra_lines, mid_point - i]:
            left_flag_445 += 1

        # 向右扫描
        # 扫描445行+额外的30行
        if 255 in img_detect_line[445:445 + Extra_lines, mid_point + i]:
            right_flag_445 += 1

        # ---------------------------------------------------------------------行标志的判断
        # 判断白色点的检测标志是否达到阈值

        # ---------------左边界
        # 445行左边界的判断
        if left_flag_445 == White_points_dec_thresholds:
            # 达到阈值，就设定为这一行的边界，这里获得的值是边界的x坐标，就是边界处于第几列
            left_boundary_445 = mid_point - i
        # 没有达到阈值，但是到达图像边缘，那么将左边界设为0
        if (left_flag_445 < White_points_dec_thresholds) and i == 319:
            left_boundary_445 = 0

        # ---------------右边界
        # 445行右边界的判断
        if right_flag_445 == White_points_dec_thresholds:
            # 达到阈值，就设定为这一行的边界，这里获得的值是边界的x坐标，就是边界处于第几列
            right_boundary_445 = mid_point + i
        # 没有达到阈值，但是到达图像边缘，那么将左边界设为0
        if (right_flag_445 < White_points_dec_thresholds) and i == 319:
            right_boundary_445 = 640

    return left_boundary_445, right_boundary_445


# 计算出巡线模式
def get_line_mode(lef_bound_445, rig_bound_445):
    if (abs(lef_bound_445 - 320) < 10) or (abs(rig_bound_445 - 320) < 10):
        print('back')
        return 0

    if lef_bound_445 != 0 and rig_bound_445 == 640:
        print('turn_right')
        return 1

    elif lef_bound_445 == 0 and rig_bound_445 != 640:
        print('turn_left')
        return 2

    elif lef_bound_445 != 0 and rig_bound_445 != 640:
        print('both line')
        return 3

    elif lef_bound_445 == 0 and rig_bound_445 == 640:
        print('None line')
        return 4


# 通过边界算出转角
def get_angle(lef_bound_445, rig_bound_445, line_mode, image_bina_draw):

    global angle_list

    # 设置用来绘制的两个点
    # 高点
    point_445 = (int((lef_bound_445 + rig_bound_445) / 2), 379)
    # 低点
    point_mid = (320, 479)

    # 根据巡线模式来判断
    if line_mode == 0:
        point_445 = (320, 379)
        pos_not = -1

    elif line_mode == 1:
        # 进行右转运算
        point_445 = (int((lef_bound_445 + lef_bound_445 + 350) / 2), 379)
        pos_not = 1

    elif line_mode == 2:
        # 进行左转运算
        point_445 = (int((rig_bound_445 - 350 + rig_bound_445) / 2), 379)
        pos_not = 1

    elif line_mode == 3:
        # 进行双线运算
        point_445 = (int((lef_bound_445 + rig_bound_445) / 2), 379)
        pos_not = 1

    elif line_mode == 4:
        # 扫描不到线直接直行
        point_445 = (320, 379)
        pos_not = 1

    else:
        # 出现了错误
        print('Wrong!!!!')
        point_445 = (320, 379)
        pos_not = 0

    # 巡线可视化
    if line_mode != 0:
        cv2.line(img=image_bina_draw,
                 pt1=point_445,
                 pt2=point_mid,
                 color=255,
                 thickness=1,
                 lineType=cv2.LINE_AA,
                 shift=0
                 )

    if line_mode == 0:
        cv2.line(img=image_bina_draw,
                 pt1=(160, 440),
                 pt2=(480, 440),
                 color=255,
                 thickness=1,
                 lineType=cv2.LINE_AA,
                 shift=0
                 )

    # 计算斜率的两条直线
    angle_x = point_445[0] - 320
    angle_y = 100
    k = float(angle_x) / float(angle_y)
    angle = math.atan(k) / np.pi * 180

    # 加上队列滞后
    angle_list.append(angle)
    angle_final = angle_list.pop(0)

    print("angle_final:", angle_final)

    return angle_final, pos_not


if __name__ == '__main__':
    # 获取到摄像头帧数
    cap_camera_wig = cv2.VideoCapture(0, cv2.CAP_V4L2)  # cv_4.2.1.30_cp38

    # 进行参数初始化操作
    init_lane_detect()
    while True:
        # 读取图片
        ret1, frame1 = cap_camera_wig.read()
        # 如果读取到了图片
        if ret1:
            # 进行畸变的矫正
            # camera_get = Straighten_the_camera(frame1)
            # 采用无畸变摄像头就不用进行矫正
            camera_get = frame1
        # 没有读取到就退出进程
        else:
            print("can't get the pic")
            sys.exit()

        # -------------------------------------------------
        # 读取图片
        image_test = camera_get
        # 对图片进行透视变换
        image_warp = warp_image(image_test)
        # 对图片进行二值化处理
        image_bina = Binary_image(image_warp)

        # ---------------------------   边界的相关测试
        left_bound_40, right_bound_40, left_bound_250, right_bound_250, left_bound_345, right_bound_345, left_bound_474, right_bound_474 = boundary_detect(
            image_bina)

        dec_line_mode = get_line_mode(left_bound_40, right_bound_40, left_bound_250, right_bound_250, left_bound_345,
                                      right_bound_345, left_bound_474, right_bound_474)

        get_angle(left_bound_250, right_bound_250, left_bound_345, right_bound_345, left_bound_474, right_bound_474,
                  dec_line_mode, image_bina)
        # get_line_mode(left_bound_40, right_bound_40, left_bound_250, right_bound_250, left_bound_345, right_bound_345,
        #               left_bound_474, right_bound_474)

        # print('left_boundary_40:', left_bound_40)
        # print('right_boundary_40:', right_bound_40)
        # print('left_boundary_250:', left_bound_250)
        # print('right_boundary_250:', right_bound_250)
        # print('left_boundary_345:', left_boundary_345)
        # print('right_boundary_345:', right_boundary_345)
        print('left_boundary_445:', left_bound_474)
        print('right_boundary_445:', right_bound_474)
        print('mid:', float(left_bound_474 + right_bound_474) / 2)

        # ---------------------------

        # 显示图片
        cv2.imshow('test_image', image_bina)
        # 等待退出指令
        key_wig = cv2.waitKey(1)

        if (key_wig & 0xFF) == ord('q'):
            print('exit')
            break

    # 释放视频资源
    cap_camera_wig.release()
    # 释放窗口
    cv2.destroyAllWindows()
